
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_arming.c
  * @author     baiyang
  * @date       2021-8-19
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <parameter/param.h>
#include <logger/task_logger.h>
#include <logger/blog_msg.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
bool fms_arming_pre_arm_checks(gp_arming * arming,bool display_failure);
bool fms_arming_run_pre_arm_checks(gp_arming * arming, bool display_failure);
void fms_arming_set_pre_arm_check(gp_arming * arming, bool b);
bool fms_arming_mandatory_checks(gp_arming * arming, bool display_failure);
bool fms_arming_arm_checks(gp_arming * arming, const ArmingMethod method);
static bool rc_calibration_checks(gp_arming * arming, bool display_failure);
/*----------------------------------variable----------------------------------*/
static struct gp_arming_ops ops = {.arm = fms_arming_arm,
                                   .disarm = fms_arming_disarm,
                                   .pre_arm_checks = fms_arming_pre_arm_checks,
                                   .arm_checks = fms_arming_arm_checks,
                                   .mandatory_checks = fms_arming_mandatory_checks,
                                   .rc_calibration_checks = rc_calibration_checks};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void fms_arming_ctor(gp_arming * arming)
{
    arming->ops = &ops;

    arming_ctor(arming);

    arming->require = PARAM_GET_INT8(ARMING, ARMING_REQUIRE);
    arming->checks_to_perform = PARAM_GET_INT32(ARMING, ARMING_CHECK);
    arming->accel_error_threshold = PARAM_GET_FLOAT(ARMING, ARMING_ACCTHRESH);
    arming->_rudder_arming = PARAM_GET_INT8(ARMING, ARMING_RUDDER);
    arming->_required_mission_items = PARAM_GET_INT32(ARMING, ARMING_MIS_ITEMS);
}

// check motor setup was successful
static bool motor_checks(gp_arming * arming, bool display_failure)
{
    // check motors initialised  correctly
    if (!fms.motors->_initialised_ok) {
        arming_check_failed(arming, display_failure, "Check firmware or FRAME_CLASS");
        return false;
    }

    // further checks enabled with parameters
    if (!arming_check_enabled(arming, ARMING_CHECK_PARAMETERS)) {
        return true;
    }

    return true;
}

static bool rc_calibration_checks(gp_arming * arming, bool display_failure)
{
    fms.ap.pre_arm_rc_check = arming_rc_checks_copter_sub(arming, display_failure)
        & arming_rc_calibration_checks(arming, display_failure);

    return fms.ap.pre_arm_rc_check;
}

// performs pre-arm checks. expects to be called at 1hz.
void fms_arming_update(gp_arming * arming)
{
    // perform pre-arm checks & display failures every 30 seconds
    static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2;
    pre_arm_display_counter++;
    
    bool display_fail = false;
    if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) {
        display_fail = true;
        pre_arm_display_counter = 0;
    }

    fms_arming_pre_arm_checks(arming, display_fail);
}

bool fms_arming_pre_arm_checks(gp_arming * arming,bool display_failure)
{
    const bool passed = fms_arming_run_pre_arm_checks(arming, display_failure);
    fms_arming_set_pre_arm_check(arming, passed);
    return passed;
}

// perform pre-arm checks
//  return true if the checks pass successfully
bool fms_arming_run_pre_arm_checks(gp_arming * arming, bool display_failure)
{
    // exit immediately if already armed
    if (fms.motors->_armed) {
        return true;
    }

    // check if motor interlock and Emergency Stop aux switches are used
    // at the same time.  This cannot be allowed.
    if (RCs_find_channel_for_option(&fms.channels, RC_MOTOR_INTERLOCK) &&
        RCs_find_channel_for_option(&fms.channels, RC_MOTOR_ESTOP)){
        arming_check_failed(arming, display_failure, "Interlock/E-Stop Conflict");
        return false;
    }

    // check if motor interlock aux switch is in use
    // if it is, switch needs to be in disabled position to arm
    // otherwise exit immediately.  This check to be repeated,
    // as state can change at any time.
    if (fms.ap.using_interlock && fms.ap.motor_interlock_switch) {
        arming_check_failed(arming, display_failure, "Motor Interlock Enabled");
    }

    // if pre arm checks are disabled run only the mandatory checks
    if (arming->checks_to_perform == 0) {
        return arming->ops->mandatory_checks(arming, display_failure);
    }

#if 0
    return parameter_checks(display_failure)
        & motor_checks(display_failure)
        & pilot_throttle_checks(display_failure)
        & oa_checks(display_failure)
        & gcs_failsafe_check(display_failure)
        & winch_checks(display_failure)
        & alt_checks(display_failure)
        & arming->ops->pre_arm_checks(arming, display_failure);
#endif

    return motor_checks(arming, display_failure)
        & arming_pre_arm_checks(arming, display_failure);
}

void fms_arming_set_pre_arm_check(gp_arming * arming, bool b)
{
    fms.ap.pre_arm_check = b;
    //AP_Notify::flags.pre_arm_check = b;
}

// mandatory checks that will be run if ARMING_CHECK is zero or arming forced
bool fms_arming_mandatory_checks(gp_arming * arming, bool display_failure)
{
    // call mandatory gps checks and update notify status because regular gps checks will not run
    //bool result = mandatory_gps_checks(display_failure);
    //AP_Notify::flags.pre_arm_gps_check = result;

    // call mandatory alt check
    //if (!alt_checks(display_failure)) {
    //    result = false;
    //}

    //return result;
    return true;
}

// arm_checks - perform final checks before arming
//  always called just before arming.  Return true if ok to arm
//  has side-effect that logging is started
bool fms_arming_arm_checks(gp_arming * arming, const ArmingMethod method)
{
#if 0
    // always check if inertial nav has started and is ready
    if (!ahrs.healthy()) {
        arming_check_failed(true, "AHRS not healthy");
        return false;
    }
#endif

    // always check if the current mode allows arming
    if (!fms.flightmode->allows_arming(method)) {
        arming_check_failed(arming, true, "Mode not armable");
        return false;
    }

    // always check motors
    //if (!motor_checks(true)) {
    //    return false;
    //}

    // if we are using motor interlock switch and it's enabled, fail to arm
    // skip check in Throw mode which takes control of the motor interlock
    if (fms.ap.using_interlock && fms.ap.motor_interlock_switch) {
        arming_check_failed(arming, true, "Motor Interlock Enabled");
        return false;
    }

#if 0
    // if we are not using Emergency Stop switch option, force Estop false to ensure motors
    // can run normally
    if (!rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)){
        SRV_Channels::set_emergency_stop(false);
        // if we are using motor Estop switch, it must not be in Estop position
    } else if (SRV_Channels::get_emergency_stop()){
        check_failed(true, "Motor Emergency Stopped");
        return false;
    }
#endif

    // succeed if arming checks are disabled
    if (arming->checks_to_perform == 0) {
        return true;
    }

#if 0
    // check lean angle
    if ((arming->checks_to_perform == ARMING_CHECK_ALL) || (arming->checks_to_perform & ARMING_CHECK_INS)) {
        //if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > fms.aparm.angle_max) {
        //    check_failed(ARMING_CHECK_INS, true, "Leaning");
        //    return false;
        //}
    }

    // check throttle
    if ((arming->checks_to_perform == ARMING_CHECK_ALL) || (arming->checks_to_perform & ARMING_CHECK_RC)) {
         #if FRAME_CONFIG == HELI_FRAME
        const char *rc_item = "Collective";
        #else
        const char *rc_item = "Throttle";
        #endif
        // check throttle is not too low - must be above failsafe throttle
        if (arming->g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) {
            check_failed(ARMING_CHECK_RC, true, "%s below failsafe", rc_item);
            return false;
        }

        // check throttle is not too high - skips checks if arming from GCS in Guided
        if (!(method == AP_Arming::Method::MAVLINK && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS))) {
            // above top of deadband is too always high
            if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) {
                check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
                return false;
            }
            // in manual modes throttle must be at zero
            #if FRAME_CONFIG != HELI_FRAME
            if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) {
                check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
                return false;
            }
            #endif
        }
    }

    // check if safety switch has been pushed
    if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
        check_failed(true, "Safety Switch");
        return false;
    }
#endif

    // superclass method should always be the last thing called; it
    // has side-effects which would need to be cleaned up if one of
    // our arm checks failed
    return arming_arm_checks(arming, method);
}

bool fms_arming_arm(gp_arming * arming, const ArmingMethod method, const bool do_arming_checks)
{
    static bool in_arm_motors = false;

    // exit immediately if already in this function
    if (in_arm_motors) {
        return false;
    }
    in_arm_motors = true;

    // return true if already armed
    if (fms.motors->_armed) {
        in_arm_motors = false;
        return true;
    }

    if (!arming_arm(arming, method, do_arming_checks)) {
        //AP_Notify::events.arming_failed = true;
        in_arm_motors = false;
        return false;
    }

    // let logger know that we're armed (it may open logs e.g.)
    //AP::logger().set_vehicle_armed(true);
    if (PARAM_GET_INT32(VEHICLE, BLOG_MODE) == 0 || 
        PARAM_GET_INT32(VEHICLE, BLOG_MODE) == 1) {
        logger_start_blog(NULL);
    }

    // disable cpu failsafe because initialising everything takes a while
    //copter.failsafe_disable();

#if 0
    // notify that arming will occur (we do this early to give plenty of warning)
    AP_Notify::flags.armed = true;
    // call notify update a few times to ensure the message gets out
    for (uint8_t i=0; i<=10; i++) {
        AP::notify().update();
    }
#endif

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    mavproxy_send_statustext(MAV_SEVERITY_INFO, "Arming motors");
#endif

    // Remember Orientation
    // --------------------
    //copter.init_simple_bearing();

    fms.initial_armed_bearing = fms.ahrs->yaw_sensor_cd;

#if 0
    if (!ahrs.home_is_set()) {
        // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
        ahrs.resetHeightDatum();
        AP::logger().Write_Event(LogEvent::EKF_ALT_RESET);

        // we have reset height, so arming height is zero
        copter.arming_altitude_m = 0;
    } else if (!ahrs.home_is_locked()) {
        // Reset home position if it has already been set before (but not locked)
        if (!copter.set_home_to_current_location(false)) {
            // ignore failure
        }

        // remember the height when we armed
        copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01;
    }
    //copter.update_super_simple_bearing(false);
#endif

    // enable output to motors
    MotorsMat_output_min((MotorsMat_HandleTypeDef *)fms.motors);

    // finally actually arm the motors
    MotorsMat_set_armed((MotorsMat_HandleTypeDef *)fms.motors,true);

    // log flight mode in case it was changed while vehicle was disarmed
    blog_write_mode(fms.flightmode->mode_number(), fms.control_mode_reason);

    // re-enable failsafe
    //copter.failsafe_enable();

    // perf monitor ignores delay due to arming
    //AP::scheduler().perf_info.ignore_this_loop();

    // flag exiting this function
    in_arm_motors = false;

    // Log time stamp of arming event
    fms.arm_time_ms = time_millis();

    // Start the arming delay
    fms.ap.in_arming_delay = true;

    // assumed armed without a arming, switch. Overridden in switches.cpp
    fms.ap.armed_with_switch = false;

    // 
    fms_actuator_armed_notify(true);

    blog_write_event(ARM, ARMED);

    // return success
    return true;
}

// arming.disarm - disarm motors
bool fms_arming_disarm(gp_arming * arming, const ArmingMethod method, bool do_disarm_checks)
{
    // return immediately if we are already disarmed
    if (!fms.motors->_armed) {
        return true;
    }

    // do not allow disarm via mavlink if we think we are flying:
    if (do_disarm_checks &&
        method == ARMING_CHECK_MAVLINK &&
        !fms.ap.land_complete) {
        return false;
    }

    if (!arming_disarm(arming, method, do_disarm_checks)) {
        return false;
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    mavproxy_send_statustext(MAV_SEVERITY_INFO, "Disarming motors");
#endif

#if 0
    auto &ahrs = AP::ahrs();

    // save compass offsets learned by the EKF if enabled
    Compass &compass = AP::compass();
    if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
        for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
            Vector3f magOffsets;
            if (ahrs.getMagOffsets(i, magOffsets)) {
                compass.set_and_save_offsets(i, magOffsets);
            }
        }
    }
#endif

#if AUTOTUNE_ENABLED == ENABLED
#if 0
    // save auto tuned parameters
    if (copter.flightmode == &copter.mode_autotune) {
        copter.mode_autotune.save_tuning_gains();
    } else {
        copter.mode_autotune.reset();
    }
#endif
#endif

    // we are not in the air
    fms_set_land_complete(true);
    fms_set_land_complete_maybe(true);

    // send disarm command to motors
    MotorsMat_set_armed((MotorsMat_HandleTypeDef *)fms.motors,false);

#if MODE_AUTO_ENABLED == ENABLED
    // reset the mission
    //copter.mode_auto.mission.reset();
#endif

    //AP::logger().set_vehicle_armed(false);

    fms.ap.in_arming_delay = false;

    // 
    fms_actuator_armed_notify(false);

    blog_write_event(ARM, DISARMED);

    if (PARAM_GET_INT32(VEHICLE, BLOG_MODE) == 0) {
        logger_stop_blog();
    }

    return true;
}

/*------------------------------------test------------------------------------*/


